/*************************************************
 *  Object:       Tric! A flying Ricotta.
 *  Authors:      Teo (matteo.piva-gmail.com)
                  Carletto (carlo.checchi-gmail.com)
*************************************************/

#include "config.h"
#include "pid.h"


void PID_update(float *RX_angle, float *SENSORS_angle, float *PID_output) {
    static float PID_err_proportional_old[3];
    static float PID_err_integral[3];
    float PID_err_proportional[3];
    float PID_err_derivative[3];
    float PID_output[3];
    unsigned short cnt;

    for (cnt=0; cnt<3; cnt++) {
        //Caculate PID errors
        PID_err_proportional[cnt] = RX_angle[cnt] - SENSORS_angle[cnt];
        if((PID_err_proportional[cnt] > PID_err_integral_min) || (PID_err_proportional[cnt] < -PID_err_integral_min)) PID_err_integral[cnt] += PID_err_proportional[cnt]* PID_processTime;
        PID_err_derivative[cnt] = (PID_err_proportional[cnt] - PID_err_proportional_old[cnt]) / PID_processTime;
        PID_output[cnt] = (PID_Kp*PID_err_proportional[cnt]) + (PID_Ki*PID_err_integral[cnt]) + (PID_Kd*PID_err_derivative[cnt]);
        //Saturation Filter
        if(PID_output[cnt] > PID_output_max) PID_output[cnt] = PID_output_max;
        else if(PID_output[cnt] < PID_output_min) PID_output[cnt] = PID_output_min;
        //Update error
        PID_err_proportional_old[cnt] = PID_err_proportional[cnt];
    }
}

void PID_applyToMotors(float *PID_output, unsigned short *MOTORS_speed) {
    unsigned short cnt;
    int motors_temp;
    
    for (cnt=0; cnt<4; cnt++) {
        switch(cnt) {
            case 0: motors_temp = (int)(PID_output[0]*1.1547 + PID_output[1]*0.5); break;
            case 1: motors_temp = (int)(-(PID_output[0]*1.1547) + PID_output[1]*0.5); break;
            case 2: motors_temp = (int)(-(PID_output[1])); break;
            case 3: motors_temp = (int)(PID_output[2]); break;
        }
        motors_temp = (int)(PID_output[0]*1.1547 + PID_output[1]*0.5);
        if (motors_temp < MOTORS_min_speed) MOTORS_speed[cnt] = MOTORS_min_speed;
        else if (motors_temp > MOTORS_max_speed) MOTORS_speed[cnt] = MOTORS_max_speed;
        else MOTORS_speed[cnt] = (unsigned short)(motors_temp);
    }
}